const byte ir_threshold = 120;
const byte ir_tx = 5;

const byte stepper_en = 13;
const byte stepper_1 = 11;
const byte stepper_2 = 12;

const byte ir_left    = A0;
const byte ir_center  = A4;
const byte ir_right   = A3;

#define DIR_FORWARD   0
#define DIR_LEFT      1
#define DIR_RIGHT     2
static volatile int last_dir = DIR_FORWARD;

void stepper_on() {
  digitalWrite(stepper_en,HIGH);
}

void stepper_off() {
  digitalWrite(stepper_en,LOW);
}

void forward() {
  digitalWrite(stepper_1,HIGH);
  digitalWrite(stepper_2,HIGH);
}

void back() {
  digitalWrite(stepper_1,LOW);
  digitalWrite(stepper_2,LOW);
}

void left() {
  digitalWrite(stepper_1,HIGH);
  digitalWrite(stepper_2,LOW);
}

void right() {
  digitalWrite(stepper_1,LOW);
  digitalWrite(stepper_2,HIGH);
}

void turn(int backtm, int roatetm) {
    back();
    delay(backtm);
    if(roatetm > 0) {
      last_dir = DIR_RIGHT;
      right();
    }
    else {
      last_dir = DIR_LEFT;
      left();
    }
    delay(abs(roatetm));
}

void setup() {
  // put your setup code here, to run once:
  pinMode(ir_tx,OUTPUT);
  pinMode(stepper_en,OUTPUT);
  pinMode(stepper_1,OUTPUT);
  pinMode(stepper_2,OUTPUT);

}

void loop() {
  int l, c, r;
  stepper_on();
  digitalWrite(ir_tx,LOW);
  delay(20);
  l = analogRead(ir_left);
  c = analogRead(ir_center);
  r = analogRead(ir_right);
  digitalWrite(ir_tx,HIGH);
  delay(20);
  l -= analogRead(ir_left);
  c -= analogRead(ir_center);
  r -= analogRead(ir_right);

  if ( l > ir_threshold && c > ir_threshold && r > ir_threshold) {
    if(last_dir == DIR_LEFT) {
      turn(1000,-300);
    }
    else if(last_dir == DIR_RIGHT)
      turn(1000,300);
    else
      turn(2000,700);
  }
  else if( l > ir_threshold && r < ir_threshold) {
    if (last_dir == DIR_LEFT) {
      turn(1000,-300);
    }
    else {
      turn(1000,300);
      last_dir = DIR_RIGHT;
    }
  }
  else if( l < ir_threshold && r > ir_threshold) {
    if (last_dir == DIR_RIGHT) {
      turn(1000,300);
    }
    else {
      turn(1000,-300);
      last_dir = DIR_LEFT;
    }
  }
  else if( c > ir_threshold) {
    if(last_dir == DIR_LEFT)
      turn(1000,-300);
    else if(last_dir == DIR_RIGHT)
      turn(1000,300);
    else
      turn(2000,700);
  }
  else {
    forward();
  }
  delay(60);
}


